#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>

// 默认地址 0x40
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

// 舵机角度
#define SERVO_0 120  // 102
#define SERVO_45 187
#define SERVO_90 280
#define SERVO_135 373
#define SERVO_180 510  // 510

// 字段对应通道
/*     1
      ----
   0 |    | 2
     | 6  |
      ----
   5 |    | 3
     | 4  |
      ----
*/
#define SEG_00 5
#define SEG_01 6
#define SEG_02 4
#define SEG_03 2
#define SEG_04 1
#define SEG_05 0
#define SEG_06 3

#define SEG_10 2 + 8
#define SEG_11 0 + 8
#define SEG_12 1 + 8
#define SEG_13 5 + 8
#define SEG_14 4 + 8
#define SEG_15 6 + 8
#define SEG_16 3 + 8
// 延时
#define DELAY_SEC 200

// our servo # counter
uint8_t servonum = 0;
char comchar;

bool seg03up = false;
bool seg05up = false;
bool seg06up = false;

bool seg13up = false;
bool seg15up = false;
bool seg16up = false;

void SetSeg06Up() {
  if (seg06up) {
    return;
  };
  seg06up = true;
  if (seg03up) {
    pwm.setPWM(SEG_03, 0, SERVO_135);
  };
  if (seg05up) {
    pwm.setPWM(SEG_05, 0, SERVO_135);
  };
  delay(DELAY_SEC);
  pwm.setPWM(SEG_06, 0, SERVO_180);
  delay(DELAY_SEC);
  if (seg03up) {
    pwm.setPWM(SEG_03, 0, SERVO_180);
  };
  if (seg05up) {
    pwm.setPWM(SEG_05, 0, SERVO_180);
  };
}

void SetSeg06Down() {
  if (!seg06up) {
    return;
  };
  seg06up = false;
  if (seg03up) {
    pwm.setPWM(SEG_03, 0, SERVO_135);
  };
  if (seg05up) {
    pwm.setPWM(SEG_05, 0, SERVO_135);
  };
  delay(DELAY_SEC);
  pwm.setPWM(SEG_06, 0, SERVO_0);
  delay(DELAY_SEC);
  if (seg03up) {
    pwm.setPWM(SEG_03, 0, SERVO_180);
  };
  if (seg05up) {
    pwm.setPWM(SEG_05, 0, SERVO_180);
  };
}

void SetSeg16Up() {
  if (seg16up) {
    return;
  };
  seg16up = true;
  if (seg13up) {
    pwm.setPWM(SEG_13, 0, SERVO_135);
  };
  if (seg15up) {
    pwm.setPWM(SEG_15, 0, SERVO_135);
  };
  delay(DELAY_SEC);
  pwm.setPWM(SEG_16, 0, SERVO_180);
  delay(DELAY_SEC);
  if (seg13up) {
    pwm.setPWM(SEG_13, 0, SERVO_180);
  };
  if (seg15up) {
    pwm.setPWM(SEG_15, 0, SERVO_180);
  };
}

void SetSeg16Down() {
  if (!seg16up) {
    return;
  };
  seg16up = false;
  if (seg13up) {
    pwm.setPWM(SEG_13, 0, SERVO_135);
  };
  if (seg15up) {
    pwm.setPWM(SEG_15, 0, SERVO_135);
  };
  delay(DELAY_SEC);
  pwm.setPWM(SEG_16, 0, SERVO_0);
  delay(DELAY_SEC);
  if (seg13up) {
    pwm.setPWM(SEG_13, 0, SERVO_180);
  };
  if (seg15up) {
    pwm.setPWM(SEG_15, 0, SERVO_180);
  };
}

void setup() {
  Serial.begin(9600);
  Serial.println("8 channel Servo test!");

  pwm.begin();
  pwm.setPWMFreq(50);  // 50HZ更新频率，相当于20ms的周期

  delay(10);

  // 复位
  seg05up = false;
  seg03up = false;
  seg06up = false;
  pwm.setPWM(SEG_00, 0, SERVO_0);
  delay(DELAY_SEC);
  pwm.setPWM(SEG_01, 0, SERVO_0);
  delay(DELAY_SEC);
  pwm.setPWM(SEG_02, 0, SERVO_0);
  delay(DELAY_SEC);
  pwm.setPWM(SEG_03, 0, SERVO_0);
  delay(DELAY_SEC);
  pwm.setPWM(SEG_04, 0, SERVO_0);
  delay(DELAY_SEC);
  pwm.setPWM(SEG_05, 0, SERVO_0);
  delay(DELAY_SEC);
  pwm.setPWM(SEG_06, 0, SERVO_0);
  delay(DELAY_SEC);
  
  seg15up = false;
  seg13up = false;
  seg16up = false;
  pwm.setPWM(SEG_10, 0, SERVO_0);
  delay(DELAY_SEC);
  pwm.setPWM(SEG_11, 0, SERVO_0);
  delay(DELAY_SEC);
  pwm.setPWM(SEG_12, 0, SERVO_0);
  delay(DELAY_SEC);
  pwm.setPWM(SEG_13, 0, SERVO_0);
  delay(DELAY_SEC);
  pwm.setPWM(SEG_14, 0, SERVO_0);
  delay(DELAY_SEC);
  pwm.setPWM(SEG_15, 0, SERVO_0);
  delay(DELAY_SEC);
  pwm.setPWM(SEG_16, 0, SERVO_0);
  delay(DELAY_SEC);
}

void loop() {
  while (Serial.available() > 0) {
    comchar = Serial.read();  //读串口第一个字节
    Serial.write(comchar);
    switch (comchar) {
      case '0':
        seg03up = true;
        seg05up = true;
        pwm.setPWM(SEG_00, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_01, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_02, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_03, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_04, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_05, 0, SERVO_180);
        delay(100);
        SetSeg06Down();
        
        seg13up = true;
        seg15up = true;
        pwm.setPWM(SEG_10, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_11, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_12, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_13, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_14, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_15, 0, SERVO_180);
        delay(100);
        SetSeg16Down();
        break;
      case '1':
        seg03up = true;
        seg05up = false;
        pwm.setPWM(SEG_00, 0, SERVO_0);
        delay(100);
        pwm.setPWM(SEG_01, 0, SERVO_0);
        delay(100);
        pwm.setPWM(SEG_02, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_03, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_04, 0, SERVO_0);
        delay(100);
        pwm.setPWM(SEG_05, 0, SERVO_0);
        delay(100);
        SetSeg06Down();
        
        seg13up = true;
        seg15up = false;
        pwm.setPWM(SEG_10, 0, SERVO_0);
        delay(100);
        pwm.setPWM(SEG_11, 0, SERVO_0);
        delay(100);
        pwm.setPWM(SEG_12, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_13, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_14, 0, SERVO_0);
        delay(100);
        pwm.setPWM(SEG_15, 0, SERVO_0);
        delay(100);
        SetSeg16Down();
        break;
      case '2':
        seg03up = false;
        seg05up = true;
        pwm.setPWM(SEG_00, 0, SERVO_0);
        delay(100);
        pwm.setPWM(SEG_01, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_02, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_03, 0, SERVO_0);
        delay(100);
        pwm.setPWM(SEG_04, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_05, 0, SERVO_180);
        delay(100);
        SetSeg06Up();
        
        seg13up = false;
        seg15up = true;
        pwm.setPWM(SEG_10, 0, SERVO_0);
        delay(100);
        pwm.setPWM(SEG_11, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_12, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_13, 0, SERVO_0);
        delay(100);
        pwm.setPWM(SEG_14, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_15, 0, SERVO_180);
        delay(100);
        SetSeg16Up();
        break;
      case '3':
        seg03up = true;
        seg05up = false;
        pwm.setPWM(SEG_00, 0, SERVO_0);
        delay(100);
        pwm.setPWM(SEG_01, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_02, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_03, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_04, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_05, 0, SERVO_0);
        delay(100);
        SetSeg06Up();
        
        seg13up = true;
        seg15up = false;
        pwm.setPWM(SEG_10, 0, SERVO_0);
        delay(100);
        pwm.setPWM(SEG_11, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_12, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_13, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_14, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_15, 0, SERVO_0);
        delay(100);
        SetSeg16Up();
        break;
      case '4':
        seg03up = true;
        seg05up = false;
        pwm.setPWM(SEG_00, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_01, 0, SERVO_0);
        delay(100);
        pwm.setPWM(SEG_02, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_03, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_04, 0, SERVO_0);
        delay(100);
        pwm.setPWM(SEG_05, 0, SERVO_0);
        delay(100);
        SetSeg06Up();
        
        seg13up = true;
        seg15up = false;
        pwm.setPWM(SEG_10, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_11, 0, SERVO_0);
        delay(100);
        pwm.setPWM(SEG_12, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_13, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_14, 0, SERVO_0);
        delay(100);
        pwm.setPWM(SEG_15, 0, SERVO_0);
        delay(100);
        SetSeg16Up();
        break;
      case '5':
        seg03up = true;
        seg05up = false;
        pwm.setPWM(SEG_00, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_01, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_02, 0, SERVO_0);
        delay(100);
        pwm.setPWM(SEG_03, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_04, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_05, 0, SERVO_0);
        delay(100);
        SetSeg06Up();
        
        seg13up = true;
        seg15up = false;
        pwm.setPWM(SEG_10, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_11, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_12, 0, SERVO_0);
        delay(100);
        pwm.setPWM(SEG_13, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_14, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_15, 0, SERVO_0);
        delay(100);
        SetSeg16Up();
        break;
      case '6':
        seg03up = true;
        seg05up = true;
        pwm.setPWM(SEG_00, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_01, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_02, 0, SERVO_0);
        delay(100);
        pwm.setPWM(SEG_03, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_04, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_05, 0, SERVO_180);
        delay(100);
        SetSeg06Up();
        
        seg13up = true;
        seg15up = true;
        pwm.setPWM(SEG_10, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_11, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_12, 0, SERVO_0);
        delay(100);
        pwm.setPWM(SEG_13, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_14, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_15, 0, SERVO_180);
        delay(100);
        SetSeg16Up();
        break;
      case '7':
        seg03up = true;
        seg05up = false;
        pwm.setPWM(SEG_00, 0, SERVO_0);
        delay(100);
        pwm.setPWM(SEG_01, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_02, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_03, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_04, 0, SERVO_0);
        delay(100);
        pwm.setPWM(SEG_05, 0, SERVO_0);
        delay(100);
        SetSeg06Down();
        
        seg13up = true;
        seg15up = false;
        pwm.setPWM(SEG_10, 0, SERVO_0);
        delay(100);
        pwm.setPWM(SEG_11, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_12, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_13, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_14, 0, SERVO_0);
        delay(100);
        pwm.setPWM(SEG_15, 0, SERVO_0);
        delay(100);
        SetSeg16Down();
        break;
      case '8':
        seg03up = true;
        seg05up = true;
        pwm.setPWM(SEG_00, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_01, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_02, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_03, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_04, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_05, 0, SERVO_180);
        delay(100);
        SetSeg06Up();
        
        seg13up = true;
        seg15up = true;
        pwm.setPWM(SEG_10, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_11, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_12, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_13, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_14, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_15, 0, SERVO_180);
        delay(100);
        SetSeg16Up();
        break;
      case '9':
        seg03up = true;
        seg05up = true;
        pwm.setPWM(SEG_00, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_01, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_02, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_03, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_04, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_05, 0, SERVO_0);
        delay(100);
        SetSeg06Up();
        
        seg13up = true;
        seg15up = true;
        pwm.setPWM(SEG_10, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_11, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_12, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_13, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_14, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_15, 0, SERVO_0);
        delay(100);
        SetSeg16Up();
        break;

      case 'd':
        seg03up = false;
        seg05up = false;
        pwm.setPWM(SEG_00, 0, SERVO_0);
        delay(100);
        pwm.setPWM(SEG_01, 0, SERVO_0);
        delay(100);
        pwm.setPWM(SEG_02, 0, SERVO_0);
        delay(100);
        pwm.setPWM(SEG_03, 0, SERVO_0);
        delay(100);
        pwm.setPWM(SEG_04, 0, SERVO_0);
        delay(100);
        pwm.setPWM(SEG_05, 0, SERVO_0);
        delay(100);
        SetSeg06Down();
        
        seg13up = false;
        seg15up = false;
        pwm.setPWM(SEG_10, 0, SERVO_0);
        delay(100);
        pwm.setPWM(SEG_11, 0, SERVO_0);
        delay(100);
        pwm.setPWM(SEG_12, 0, SERVO_0);
        delay(100);
        pwm.setPWM(SEG_13, 0, SERVO_0);
        delay(100);
        pwm.setPWM(SEG_14, 0, SERVO_0);
        delay(100);
        pwm.setPWM(SEG_15, 0, SERVO_0);
        delay(100);
        SetSeg16Down();
        break;

      case 'm':
        pwm.setPWM(SEG_00, 0, SERVO_90);
        delay(100);
        pwm.setPWM(SEG_01, 0, SERVO_90);
        delay(100);
        pwm.setPWM(SEG_02, 0, SERVO_90);
        delay(100);
        pwm.setPWM(SEG_03, 0, SERVO_90);
        delay(100);
        pwm.setPWM(SEG_04, 0, SERVO_90);
        delay(100);
        pwm.setPWM(SEG_05, 0, SERVO_90);
        delay(100);
        pwm.setPWM(SEG_06, 0, SERVO_90);
        delay(100);

        
        pwm.setPWM(SEG_10, 0, SERVO_90);
        delay(100);
        pwm.setPWM(SEG_11, 0, SERVO_90);
        delay(100);
        pwm.setPWM(SEG_12, 0, SERVO_90);
        delay(100);
        pwm.setPWM(SEG_13, 0, SERVO_90);
        delay(100);
        pwm.setPWM(SEG_14, 0, SERVO_90);
        delay(100);
        pwm.setPWM(SEG_15, 0, SERVO_90);
        delay(100);
        pwm.setPWM(SEG_16, 0, SERVO_90);
        delay(100);
        break;

      case 'u':
        seg03up = true;
        seg05up = true;
        pwm.setPWM(SEG_00, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_01, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_02, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_03, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_04, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_05, 0, SERVO_180);
        delay(100);
        SetSeg06Up();

        
        seg13up = true;
        seg15up = true;
        pwm.setPWM(SEG_10, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_11, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_12, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_13, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_14, 0, SERVO_180);
        delay(100);
        pwm.setPWM(SEG_15, 0, SERVO_180);
        delay(100);
        SetSeg16Up();
        break;

      default:
        break;
    }
  }
}
